#include "PointMass.h"

namespace BlobSalladDS
{
	PointMass::PointMass(double cx, double cy, double mass)
	{
		this->cur = new Vector(cx, cy);
		this->prev = new Vector(cx, cy);
		this->mass = mass;
		this->force = new Vector(0, 0);
		this->result = new Vector(0, 0);
		this->friction = 0.01;
	}

	double PointMass::getXPos()
	{
		return this->cur->getX();
	}

	double PointMass::getYPos()
	{
		return this->cur->getY();
	}

	Vector *PointMass::getPos()
	{
		return this->cur;
	}

	double PointMass::getXPrevPos()
	{
		return this->prev->getX();
	}

	double PointMass::getYPrevPos()
	{
		return this->prev->getY();
	}

	Vector *PointMass::getPrevPos()
	{
		return this->prev;
	}

	void PointMass::addXPos(double dx)
	{
		this->cur->addX(dx);
	}

	void PointMass::addYPos(double dy)
	{
		this->cur->addY(dy);
	}

	void PointMass::setForce(Vector &force)
	{
		this->force->set(force);
	}

	void PointMass::addForce(Vector &force)
	{
		this->force->add(force);
	}

	double PointMass::getMass()
	{
		return this->mass;
	}

	void PointMass::setMass(double mass)
	{
		this->mass = mass;
	}

	void PointMass::move(double dt)
	{
		double t, a, c, dtdt;

		dtdt = dt * dt;

		a = this->force->getX() / this->mass;
		c = this->cur->getX();
		t = (2.0 - this->friction) * c - (1.0 - this->friction) * this->prev->getX() + a * dtdt;
		this->prev->setX(c);
		this->cur->setX(t);

		a = this->force->getY() / this->mass;
		c = this->cur->getY();
		t = (2.0 - this->friction) * c - (1.0 - this->friction) * this->prev->getY() + a * dtdt;
		this->prev->setY(c);
		this->cur->setY(t);
	}

	void PointMass::setFriction(double friction)
	{
		this->friction = friction;
	}

	double PointMass::getVelocity()
	{
		double cXpX, cYpY;

		cXpX = this->cur->getX() - this->prev->getX();
		cYpY = this->cur->getY() - this->prev->getY();

		return cXpX * cXpX + cYpY * cYpY;
	}

	void PointMass::draw(void *ctx, double scaleFactor)
	{
		//TODO: draw something using PALib
	}
}
